#include "board.h"
#include "tp2855.h"
extern void
logmsg( const char *fmt, ... );
int TP2855_common_test(tw2855_context_t* ctx)
{
		/* Disable MIPI CSI2 output */
		tp28xx_write_reg(ctx,0x40, MIPI_PAGE);
		tp28xx_write_reg(ctx,0x23, 0x02);
				
		//TP2855_decoder_init(ctx,CH_ALL,HD30,STD_TVI);
		TP2855_decoder_init(ctx,CH_ALL, HD30, STD_HDA);
		if (ctx->platform == IMX6X_PLAT_IMX6D) {
			//TP2855_mipi_out(ctx,MIPI_4CH4LANE_297M);
			TP2855_mipi_out(ctx,MIPI_4CH2LANE_594M);
		}
		else {
			TP2855_mipi_out(ctx,MIPI_4CH2LANE_594M);
		}
		printf("---Run in TP2855 common test mode---\n");
		return 0;
}

int TP2855_enable_mipi_clk(void * ctx, uint8_t _enable){
	struct _tw2855_context *decoder_ctx =
			(struct _tw2855_context *)ctx;
	if(_enable){
		/* Enable MIPI CSI2 clock output */
		tp28xx_write_reg(decoder_ctx, 0x40, MIPI_PAGE);
		tp28xx_write_reg(decoder_ctx, 0x23, 0x00);
	}
	else{
		/* Disable MIPI CSI2 clock output */
		tp28xx_write_reg(decoder_ctx, 0x40, MIPI_PAGE);
		tp28xx_write_reg(decoder_ctx, 0x23, 0x02);
	}
	return (0);
}

int TP2855_common_init(tw2855_context_t* ctx, uint8_t mipi_lanes)
{
		/* Disable MIPI CSI2 output */
		tp28xx_write_reg(ctx,0x40, MIPI_PAGE);
		tp28xx_write_reg(ctx,0x23, 0x02);
		if(ctx->std == SD_PAL){
			TP2855_decoder_init(ctx,CH_ALL,PAL,STD_TVI);
			logmsg( "Init tp2855 to [TP2855-PAL25-720]\n");
		}else if(ctx->std == HD_720P){
			if(ctx->fps == 30){
				TP2855_decoder_init(ctx,CH_ALL,HD30,STD_HDA);
				logmsg( "Init tp2855 to [TP2855-AHD30-720P]\n");
			}else if(ctx->fps == 25){
				TP2855_decoder_init(ctx,CH_ALL,HD25,STD_HDA);
				logmsg( "Init tp2855 to [TP2855-AHD25-720P]\n");
			}
			else{
				logmsg( "Please check fps.\n");
			}
		}else{
			logmsg( "Please check std.\n");
		}
        /*For zewin AHD camera, we use HD25 & STD_HDA*/
//		TP2855_decoder_init(ctx,CH_ALL,HD25,STD_HDA);
		/*For zoomlion 10.1 AHD camera, we use HD30 & STD_HDA*/
		//TP2855_decoder_init(ctx,CH_ALL, HD30, STD_HDA);

		//PAL
//		TP2855_decoder_init(ctx,CH_ALL,PAL,STD_TVI);
		//NTSC
//		TP2855_decoder_init(ctx,CH_ALL,HD30,STD_HDA);

		if(mipi_lanes == 2){
			TP2855_mipi_out(ctx, MIPI_4CH2LANE_594M);
		}
		else{
			TP2855_mipi_out(ctx, MIPI_4CH4LANE_297M);
		}
		return 0;
}
/////////////////////////////////
//ch: video channel
//fmt: PAL/NTSC/HD25/HD30...
//std: STD_TVI/STD_HDA
////////////////////////////////
void TP2855_decoder_init(tw2855_context_t* ctx,unsigned char ch,
		unsigned char fmt,unsigned char std)
{
	unsigned char tmp;
	const unsigned char SYS_MODE[5]={0x01,0x02,0x04,0x08,0x0f}; 
	tp28xx_write_reg(ctx,0x40, ch);
	//patch to 2021-01-27
	tp28xx_write_reg(ctx,0x45, 0x01);
	tp28xx_write_reg(ctx,0x06, 0x12); //default value
	//patch to 2021-01-27 end
	tp28xx_write_reg(ctx,0x26, 0x04);
	//Force in test mode
	//tp28xx_write_reg(ctx,0x2A, 0x3C);
	if(PAL == fmt)
	{
		logmsg( "Channel %d run in PAL format\n",ch);
		tmp = tp28xx_read_reg(ctx,0xf5);
		tmp |= SYS_MODE[ch];
		tp28xx_write_reg(ctx,0xf5, tmp);
		//patch to 2021-01-27
		tp28xx_write_reg(ctx,0x06, 0x32);
		//patch to 2021-01-27 end
		tp28xx_write_reg(ctx,0x02, 0x47);
		tp28xx_write_reg(ctx,0x0c, 0x13);
		tp28xx_write_reg(ctx,0x0d, 0x51);

		tp28xx_write_reg(ctx,0x15, 0x03);
		tp28xx_write_reg(ctx,0x16, 0xf0); 
		tp28xx_write_reg(ctx,0x17, 0xa0); 
		tp28xx_write_reg(ctx,0x18, 0x17);
		tp28xx_write_reg(ctx,0x19, 0x20);
		tp28xx_write_reg(ctx,0x1a, 0x15);				
		tp28xx_write_reg(ctx,0x1c, 0x06);
		tp28xx_write_reg(ctx,0x1d, 0xc0);
	
		tp28xx_write_reg(ctx,0x20, 0x48);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x37);
		tp28xx_write_reg(ctx,0x23, 0x3f);

		tp28xx_write_reg(ctx,0x2b, 0x70);
		tp28xx_write_reg(ctx,0x2c, 0x2a);
		tp28xx_write_reg(ctx,0x2d, 0x4b);
		tp28xx_write_reg(ctx,0x2e, 0x56);

		tp28xx_write_reg(ctx,0x30, 0x7a);
		tp28xx_write_reg(ctx,0x31, 0x4a);
		tp28xx_write_reg(ctx,0x32, 0x4d);
		tp28xx_write_reg(ctx,0x33, 0xfb);	
		
		tp28xx_write_reg(ctx,0x35, 0x65); 
		tp28xx_write_reg(ctx,0x38, 0x00);
		tp28xx_write_reg(ctx,0x39, 0x04);
	}
	else if(NTSC == fmt)
	{
		tmp = tp28xx_read_reg(ctx,0xf5);
		tmp |= SYS_MODE[ch];
		tp28xx_write_reg(ctx,0xf5, tmp);
				
		tp28xx_write_reg(ctx,0x02, 0x47);
		tp28xx_write_reg(ctx,0x0c, 0x13);
		tp28xx_write_reg(ctx,0x0d, 0x50);

		tp28xx_write_reg(ctx,0x15, 0x03);
		tp28xx_write_reg(ctx,0x16, 0xd6);
		tp28xx_write_reg(ctx,0x17, 0xa0);
		tp28xx_write_reg(ctx,0x18, 0x12);
		tp28xx_write_reg(ctx,0x19, 0xf0);
		tp28xx_write_reg(ctx,0x1a, 0x05);
		tp28xx_write_reg(ctx,0x1c, 0x06);
		tp28xx_write_reg(ctx,0x1d, 0xb4);
	
		tp28xx_write_reg(ctx,0x20, 0x40);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x2b, 0x70);
		tp28xx_write_reg(ctx,0x2c, 0x2a);
		tp28xx_write_reg(ctx,0x2d, 0x4b);
		tp28xx_write_reg(ctx,0x2e, 0x57);

		tp28xx_write_reg(ctx,0x30, 0x62);
		tp28xx_write_reg(ctx,0x31, 0xbb);
		tp28xx_write_reg(ctx,0x32, 0x96);
		tp28xx_write_reg(ctx,0x33, 0xcb);
		
		tp28xx_write_reg(ctx,0x35, 0x65);
		tp28xx_write_reg(ctx,0x38, 0x00);
		tp28xx_write_reg(ctx,0x39, 0x04);
	}
	else if(HD25 == fmt)
	{
		tmp = tp28xx_read_reg(ctx,0xf5);
		tmp |= SYS_MODE[ch];
		tp28xx_write_reg(ctx,0xf5, tmp);
				
		tp28xx_write_reg(ctx,0x02, 0x42);
		tp28xx_write_reg(ctx,0x07, 0xc0);
		tp28xx_write_reg(ctx,0x0b, 0xc0);
		tp28xx_write_reg(ctx,0x0c, 0x13);
		tp28xx_write_reg(ctx,0x0d, 0x50);

		tp28xx_write_reg(ctx,0x15, 0x13);
		tp28xx_write_reg(ctx,0x16, 0x15);
		tp28xx_write_reg(ctx,0x17, 0x00);
		tp28xx_write_reg(ctx,0x18, 0x19);
		tp28xx_write_reg(ctx,0x19, 0xd0);
		tp28xx_write_reg(ctx,0x1a, 0x25);
		tp28xx_write_reg(ctx,0x1c, 0x07);  //1280*720, 25fps
		tp28xx_write_reg(ctx,0x1d, 0xbc);  //1280*720, 25fps

		tp28xx_write_reg(ctx,0x20, 0x30);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x2b, 0x60);
		tp28xx_write_reg(ctx,0x2c, 0x0a);
		tp28xx_write_reg(ctx,0x2d, 0x30);
		tp28xx_write_reg(ctx,0x2e, 0x70);

		tp28xx_write_reg(ctx,0x30, 0x48);
		tp28xx_write_reg(ctx,0x31, 0xbb);
		tp28xx_write_reg(ctx,0x32, 0x2e);
		tp28xx_write_reg(ctx,0x33, 0x90);
		
		tp28xx_write_reg(ctx,0x35, 0x25);
		tp28xx_write_reg(ctx,0x38, 0x00);
		tp28xx_write_reg(ctx,0x39, 0x18);

		if(STD_HDA == std)
		{
    	tp28xx_write_reg(ctx,0x02, 0x46);

    	tp28xx_write_reg(ctx,0x0d, 0x71);

    	tp28xx_write_reg(ctx,0x18, 0x1b);
    	tp28xx_write_reg(ctx,0x20, 0x40);
    	tp28xx_write_reg(ctx,0x21, 0x46);

    	tp28xx_write_reg(ctx,0x25, 0xfe);
    	tp28xx_write_reg(ctx,0x26, 0x01);

    	tp28xx_write_reg(ctx,0x2c, 0x3a);
    	tp28xx_write_reg(ctx,0x2d, 0x5a);
    	tp28xx_write_reg(ctx,0x2e, 0x40);

    	tp28xx_write_reg(ctx,0x30, 0x9e);
    	tp28xx_write_reg(ctx,0x31, 0x20);
    	tp28xx_write_reg(ctx,0x32, 0x10);
    	tp28xx_write_reg(ctx,0x33, 0x90);
		}	
	}
	else if(HD30 == fmt)
	{
		tmp = tp28xx_read_reg(ctx,0xf5);
		tmp |= SYS_MODE[ch];
		tp28xx_write_reg(ctx,0xf5, tmp);//sysclock 74.25M
				
		tp28xx_write_reg(ctx,0x02, 0x42);
		tp28xx_write_reg(ctx,0x07, 0xc0);
		tp28xx_write_reg(ctx,0x0b, 0xc0);
		tp28xx_write_reg(ctx,0x0c, 0x13);
		tp28xx_write_reg(ctx,0x0d, 0x50);

		tp28xx_write_reg(ctx,0x15, 0x13);
		tp28xx_write_reg(ctx,0x16, 0x15);
		tp28xx_write_reg(ctx,0x17, 0x00);
		tp28xx_write_reg(ctx,0x18, 0x19);
		tp28xx_write_reg(ctx,0x19, 0xd0);
		tp28xx_write_reg(ctx,0x1a, 0x25);
		tp28xx_write_reg(ctx,0x1c, 0x06);  //1280*720, 30fps
		tp28xx_write_reg(ctx,0x1d, 0x72);  //1280*720, 30fps

		tp28xx_write_reg(ctx,0x20, 0x30);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x2b, 0x60);
		tp28xx_write_reg(ctx,0x2c, 0x0a);
		tp28xx_write_reg(ctx,0x2d, 0x30);
		tp28xx_write_reg(ctx,0x2e, 0x70);

		tp28xx_write_reg(ctx,0x30, 0x48);
		tp28xx_write_reg(ctx,0x31, 0xbb);
		tp28xx_write_reg(ctx,0x32, 0x2e);
		tp28xx_write_reg(ctx,0x33, 0x90);
		
		tp28xx_write_reg(ctx,0x35, 0x25);
		tp28xx_write_reg(ctx,0x38, 0x00);
		tp28xx_write_reg(ctx,0x39, 0x18);

		if(STD_HDA == std)
		{
    	tp28xx_write_reg(ctx,0x02, 0x46);

    	tp28xx_write_reg(ctx,0x0d, 0x70);

    	tp28xx_write_reg(ctx,0x18, 0x1b);
    	tp28xx_write_reg(ctx,0x20, 0x40);
    	tp28xx_write_reg(ctx,0x21, 0x46);

    	tp28xx_write_reg(ctx,0x25, 0xfe);
    	tp28xx_write_reg(ctx,0x26, 0x01);

    	tp28xx_write_reg(ctx,0x2c, 0x3a);
    	tp28xx_write_reg(ctx,0x2d, 0x5a);
    	tp28xx_write_reg(ctx,0x2e, 0x40);

    	tp28xx_write_reg(ctx,0x30, 0x9d);
    	tp28xx_write_reg(ctx,0x31, 0xca);
    	tp28xx_write_reg(ctx,0x32, 0x01);
    	tp28xx_write_reg(ctx,0x33, 0xd0);
		}	
	}
	else if(FHD30 == fmt)
	{
		tmp = tp28xx_read_reg(ctx,0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(ctx,0xf5, tmp);
		
		tp28xx_write_reg(ctx,0x02, 0x40);
		tp28xx_write_reg(ctx,0x07, 0xc0);
		tp28xx_write_reg(ctx,0x0b, 0xc0);
		tp28xx_write_reg(ctx,0x0c, 0x03);
		tp28xx_write_reg(ctx,0x0d, 0x50);

		tp28xx_write_reg(ctx,0x15, 0x03);
		tp28xx_write_reg(ctx,0x16, 0xd2);
		tp28xx_write_reg(ctx,0x17, 0x80);
		tp28xx_write_reg(ctx,0x18, 0x29);
		tp28xx_write_reg(ctx,0x19, 0x38);
		tp28xx_write_reg(ctx,0x1a, 0x47);
		tp28xx_write_reg(ctx,0x1c, 0x08);  //1920*1080, 30fps
		tp28xx_write_reg(ctx,0x1d, 0x98);  //
	
		tp28xx_write_reg(ctx,0x20, 0x30);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x2b, 0x60);
		tp28xx_write_reg(ctx,0x2c, 0x0a);
		tp28xx_write_reg(ctx,0x2d, 0x30);
		tp28xx_write_reg(ctx,0x2e, 0x70);

		tp28xx_write_reg(ctx,0x30, 0x48);
		tp28xx_write_reg(ctx,0x31, 0xbb);
		tp28xx_write_reg(ctx,0x32, 0x2e);
		tp28xx_write_reg(ctx,0x33, 0x90);
			
		tp28xx_write_reg(ctx,0x35, 0x05);
		tp28xx_write_reg(ctx,0x38, 0x00);
		tp28xx_write_reg(ctx,0x39, 0x1C);
	
		if(STD_HDA == std)
		{
    			tp28xx_write_reg(ctx,0x02, 0x44);

    			tp28xx_write_reg(ctx,0x0d, 0x72);
    
    			tp28xx_write_reg(ctx,0x15, 0x01);
    			tp28xx_write_reg(ctx,0x16, 0xf0);
    			tp28xx_write_reg(ctx,0x18, 0x2a);
    
    			tp28xx_write_reg(ctx,0x20, 0x38);
    			tp28xx_write_reg(ctx,0x21, 0x46);

    			tp28xx_write_reg(ctx,0x25, 0xfe);
    			tp28xx_write_reg(ctx,0x26, 0x0d);

    			tp28xx_write_reg(ctx,0x2c, 0x3a);
    			tp28xx_write_reg(ctx,0x2d, 0x54);
    			tp28xx_write_reg(ctx,0x2e, 0x40);

    			tp28xx_write_reg(ctx,0x30, 0xa5);
    			tp28xx_write_reg(ctx,0x31, 0x95);
    			tp28xx_write_reg(ctx,0x32, 0xe0);
    			tp28xx_write_reg(ctx,0x33, 0x60);
		}
	}
	else if(FHD25 == fmt)
	{
		tmp = tp28xx_read_reg(ctx,0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(ctx,0xf5, tmp);
		
		tp28xx_write_reg(ctx,0x02, 0x40);
		tp28xx_write_reg(ctx,0x07, 0xc0);
		tp28xx_write_reg(ctx,0x0b, 0xc0);
		tp28xx_write_reg(ctx,0x0c, 0x03);
		tp28xx_write_reg(ctx,0x0d, 0x50);
  		
		tp28xx_write_reg(ctx,0x15, 0x03);
		tp28xx_write_reg(ctx,0x16, 0xd2);
		tp28xx_write_reg(ctx,0x17, 0x80);
		tp28xx_write_reg(ctx,0x18, 0x29);
		tp28xx_write_reg(ctx,0x19, 0x38);
		tp28xx_write_reg(ctx,0x1a, 0x47);
  		
		tp28xx_write_reg(ctx,0x1c, 0x0a);  //1920*1080, 25fps
		tp28xx_write_reg(ctx,0x1d, 0x50);  //
			
		tp28xx_write_reg(ctx,0x20, 0x30);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);
  		
		tp28xx_write_reg(ctx,0x2b, 0x60);
		tp28xx_write_reg(ctx,0x2c, 0x0a);
		tp28xx_write_reg(ctx,0x2d, 0x30);
		tp28xx_write_reg(ctx,0x2e, 0x70);
  		
		tp28xx_write_reg(ctx,0x30, 0x48);
		tp28xx_write_reg(ctx,0x31, 0xbb);
		tp28xx_write_reg(ctx,0x32, 0x2e);
		tp28xx_write_reg(ctx,0x33, 0x90);
					
		tp28xx_write_reg(ctx,0x35, 0x05);
		tp28xx_write_reg(ctx,0x38, 0x00);
		tp28xx_write_reg(ctx,0x39, 0x1C);

		if(STD_HDA == std)                    
		{
   			tp28xx_write_reg(ctx,0x02, 0x44);
   			
   			tp28xx_write_reg(ctx,0x0d, 0x73);
   			
   			tp28xx_write_reg(ctx,0x15, 0x01);
   			tp28xx_write_reg(ctx,0x16, 0xf0);
    		tp28xx_write_reg(ctx,0x18, 0x2a);
   			
   			tp28xx_write_reg(ctx,0x20, 0x3c);
   			tp28xx_write_reg(ctx,0x21, 0x46);
   			
   			tp28xx_write_reg(ctx,0x25, 0xfe);
   			tp28xx_write_reg(ctx,0x26, 0x0d);
   			
   			tp28xx_write_reg(ctx,0x2c, 0x3a);
   			tp28xx_write_reg(ctx,0x2d, 0x54);
   			tp28xx_write_reg(ctx,0x2e, 0x40);
   			
   			tp28xx_write_reg(ctx,0x30, 0xa5);
   			tp28xx_write_reg(ctx,0x31, 0x86);
   			tp28xx_write_reg(ctx,0x32, 0xfb);
   			tp28xx_write_reg(ctx,0x33, 0x60);
		}
   			
	}
	else if(FHD60 == fmt)
	{
		tmp = tp28xx_read_reg(ctx,0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(ctx,0xf5, tmp);
		
		tp28xx_write_reg(ctx,0x02, 0x40);
		tp28xx_write_reg(ctx,0x07, 0xc0);
		tp28xx_write_reg(ctx,0x0b, 0xc0);
		tp28xx_write_reg(ctx,0x0c, 0x03);
		tp28xx_write_reg(ctx,0x0d, 0x50);

		tp28xx_write_reg(ctx,0x15, 0x03);
		tp28xx_write_reg(ctx,0x16, 0xf0);
		tp28xx_write_reg(ctx,0x17, 0x80);
		tp28xx_write_reg(ctx,0x18, 0x12);
		tp28xx_write_reg(ctx,0x19, 0x38);
		tp28xx_write_reg(ctx,0x1a, 0x47);
		tp28xx_write_reg(ctx,0x1c, 0x08);  //
		tp28xx_write_reg(ctx,0x1d, 0x96);  //
	
		tp28xx_write_reg(ctx,0x20, 0x38);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x27, 0xad);
		    
		tp28xx_write_reg(ctx,0x2b, 0x60);
		tp28xx_write_reg(ctx,0x2c, 0x0a);
		tp28xx_write_reg(ctx,0x2d, 0x40);
		tp28xx_write_reg(ctx,0x2e, 0x70);

		tp28xx_write_reg(ctx,0x30, 0x74);
		tp28xx_write_reg(ctx,0x31, 0x9b);
		tp28xx_write_reg(ctx,0x32, 0xa5);
		tp28xx_write_reg(ctx,0x33, 0xe0);
			
		tp28xx_write_reg(ctx,0x35, 0x05);
		tp28xx_write_reg(ctx,0x38, 0x40);
		tp28xx_write_reg(ctx,0x39, 0x68);

	}
	else if(FHD50 == fmt)
	{
		tmp = tp28xx_read_reg(ctx,0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(ctx,0xf5, tmp);

		tp28xx_write_reg(ctx,0x02, 0x40);
		tp28xx_write_reg(ctx,0x07, 0xc0);
		tp28xx_write_reg(ctx,0x0b, 0xc0);
		tp28xx_write_reg(ctx,0x0c, 0x03);
		tp28xx_write_reg(ctx,0x0d, 0x50);

		tp28xx_write_reg(ctx,0x15, 0x03);
		tp28xx_write_reg(ctx,0x16, 0xe2);
		tp28xx_write_reg(ctx,0x17, 0x80);
		tp28xx_write_reg(ctx,0x18, 0x27);
		tp28xx_write_reg(ctx,0x19, 0x38);
		tp28xx_write_reg(ctx,0x1a, 0x47);

		tp28xx_write_reg(ctx,0x1c, 0x0a);  //
		tp28xx_write_reg(ctx,0x1d, 0x4e);  //

		tp28xx_write_reg(ctx,0x20, 0x38);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x27, 0xad);

		tp28xx_write_reg(ctx,0x2b, 0x60);
		tp28xx_write_reg(ctx,0x2c, 0x0a);
		tp28xx_write_reg(ctx,0x2d, 0x40);
		tp28xx_write_reg(ctx,0x2e, 0x70);

		tp28xx_write_reg(ctx,0x30, 0x74);
		tp28xx_write_reg(ctx,0x31, 0x9b);
		tp28xx_write_reg(ctx,0x32, 0xa5);
		tp28xx_write_reg(ctx,0x33, 0xe0);

		tp28xx_write_reg(ctx,0x35, 0x05);
		tp28xx_write_reg(ctx,0x38, 0x40);
		tp28xx_write_reg(ctx,0x39, 0x68);

	}		
	else if(QHD30 == fmt)
	{
		tmp = tp28xx_read_reg(ctx,0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(ctx,0xf5, tmp);

		tp28xx_write_reg(ctx,0x02, 0x50);
		tp28xx_write_reg(ctx,0x07, 0xc0);
		tp28xx_write_reg(ctx,0x0b, 0xc0);
		tp28xx_write_reg(ctx,0x0c, 0x03);
		tp28xx_write_reg(ctx,0x0d, 0x50);

		tp28xx_write_reg(ctx,0x15, 0x23);
		tp28xx_write_reg(ctx,0x16, 0x1b);
		tp28xx_write_reg(ctx,0x17, 0x00);
		tp28xx_write_reg(ctx,0x18, 0x38);
		tp28xx_write_reg(ctx,0x19, 0xa0);
		tp28xx_write_reg(ctx,0x1a, 0x5a);
		tp28xx_write_reg(ctx,0x1c, 0x0c);  //2560*1440, 30fps
		tp28xx_write_reg(ctx,0x1d, 0xe2);  //

		tp28xx_write_reg(ctx,0x20, 0x50);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x27, 0xad);

		tp28xx_write_reg(ctx,0x2b, 0x60);
		tp28xx_write_reg(ctx,0x2c, 0x0a);
		tp28xx_write_reg(ctx,0x2d, 0x58);
		tp28xx_write_reg(ctx,0x2e, 0x70);

		tp28xx_write_reg(ctx,0x30, 0x74);
		tp28xx_write_reg(ctx,0x31, 0x58);
		tp28xx_write_reg(ctx,0x32, 0x9f);
		tp28xx_write_reg(ctx,0x33, 0x60);

		tp28xx_write_reg(ctx,0x35, 0x15);
		tp28xx_write_reg(ctx,0x36, 0xdc);
		tp28xx_write_reg(ctx,0x38, 0x40);
		tp28xx_write_reg(ctx,0x39, 0x48);
	
	}
	else if(QHD25 == fmt)
	{
		tmp = tp28xx_read_reg(ctx,0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(ctx,0xf5, tmp);

		tp28xx_write_reg(ctx,0x02, 0x50);
		tp28xx_write_reg(ctx,0x07, 0xc0);
		tp28xx_write_reg(ctx,0x0b, 0xc0);
		tp28xx_write_reg(ctx,0x0c, 0x03);
		tp28xx_write_reg(ctx,0x0d, 0x50);

		tp28xx_write_reg(ctx,0x15, 0x23);
		tp28xx_write_reg(ctx,0x16, 0x1b);
		tp28xx_write_reg(ctx,0x17, 0x00);
		tp28xx_write_reg(ctx,0x18, 0x38);
		tp28xx_write_reg(ctx,0x19, 0xa0);
		tp28xx_write_reg(ctx,0x1a, 0x5a);
		tp28xx_write_reg(ctx,0x1c, 0x0f);  //2560*1440, 25fps
		tp28xx_write_reg(ctx,0x1d, 0x76);  //

		tp28xx_write_reg(ctx,0x20, 0x50);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x27, 0xad);

		tp28xx_write_reg(ctx,0x2b, 0x60);
		tp28xx_write_reg(ctx,0x2c, 0x0a);
		tp28xx_write_reg(ctx,0x2d, 0x58);
		tp28xx_write_reg(ctx,0x2e, 0x70);

		tp28xx_write_reg(ctx,0x30, 0x74);
		tp28xx_write_reg(ctx,0x31, 0x58);
		tp28xx_write_reg(ctx,0x32, 0x9f);
		tp28xx_write_reg(ctx,0x33, 0x60);

		tp28xx_write_reg(ctx,0x35, 0x15);
		tp28xx_write_reg(ctx,0x36, 0xdc);
		tp28xx_write_reg(ctx,0x38, 0x40);
		tp28xx_write_reg(ctx,0x39, 0x48);

	}
	else if(UVGA25 == fmt) //960P25
	{

		tp28xx_write_reg(ctx,0xf5, 0xf0);

		tp28xx_write_reg(ctx,0x02, 0x42);
		tp28xx_write_reg(ctx,0x07, 0xc0);
		tp28xx_write_reg(ctx,0x0b, 0xc0);
		tp28xx_write_reg(ctx,0x0c, 0x13);
		tp28xx_write_reg(ctx,0x0d, 0x50);

		tp28xx_write_reg(ctx,0x15, 0x13);
		tp28xx_write_reg(ctx,0x16, 0x16);
		tp28xx_write_reg(ctx,0x17, 0x00);
		tp28xx_write_reg(ctx,0x18, 0xa0);
		tp28xx_write_reg(ctx,0x19, 0xc0);
		tp28xx_write_reg(ctx,0x1a, 0x35);
		tp28xx_write_reg(ctx,0x1c, 0x07);  //
		tp28xx_write_reg(ctx,0x1d, 0xbc);  //

		tp28xx_write_reg(ctx,0x20, 0x30);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x2b, 0x60);
		tp28xx_write_reg(ctx,0x2c, 0x0a);
		tp28xx_write_reg(ctx,0x2d, 0x30);
		tp28xx_write_reg(ctx,0x2e, 0x70);

		tp28xx_write_reg(ctx,0x30, 0x48);
		tp28xx_write_reg(ctx,0x31, 0xba);
		tp28xx_write_reg(ctx,0x32, 0x2e);
		tp28xx_write_reg(ctx,0x33, 0x90);

		tp28xx_write_reg(ctx,0x35, 0x14);
		tp28xx_write_reg(ctx,0x36, 0x65);
		tp28xx_write_reg(ctx,0x38, 0x00);
		tp28xx_write_reg(ctx,0x39, 0x1c);
	}
	else if(UVGA30 == fmt) //960P30
	{
		tp28xx_write_reg(ctx,0xf5, 0xf0);

		tp28xx_write_reg(ctx,0x02, 0x42);
		tp28xx_write_reg(ctx,0x07, 0xc0);
		tp28xx_write_reg(ctx,0x0b, 0xc0);
		tp28xx_write_reg(ctx,0x0c, 0x13);
		tp28xx_write_reg(ctx,0x0d, 0x50);

		tp28xx_write_reg(ctx,0x15, 0x13);
		tp28xx_write_reg(ctx,0x16, 0x16); 
		tp28xx_write_reg(ctx,0x17, 0x00); 
		tp28xx_write_reg(ctx,0x18, 0xa0);
		tp28xx_write_reg(ctx,0x19, 0xc0);
		tp28xx_write_reg(ctx,0x1a, 0x35);
		tp28xx_write_reg(ctx,0x1c, 0x06);  //
		tp28xx_write_reg(ctx,0x1d, 0x72);  //

		tp28xx_write_reg(ctx,0x20, 0x30);  
		tp28xx_write_reg(ctx,0x21, 0x84); 
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x2b, 0x60);  
		tp28xx_write_reg(ctx,0x2c, 0x0a); 
		tp28xx_write_reg(ctx,0x2d, 0x30);
		tp28xx_write_reg(ctx,0x2e, 0x70);

		tp28xx_write_reg(ctx,0x30, 0x43);  
		tp28xx_write_reg(ctx,0x31, 0x3b); 
		tp28xx_write_reg(ctx,0x32, 0x79);
		tp28xx_write_reg(ctx,0x33, 0x90);

		tp28xx_write_reg(ctx,0x35, 0x14); 
		tp28xx_write_reg(ctx,0x36, 0x65); 		
		tp28xx_write_reg(ctx,0x38, 0x00);	
		tp28xx_write_reg(ctx,0x39, 0x1c); 		
	}
		
	else if(HD30HDR == fmt)
	{

		tp28xx_write_reg(ctx,0xf5, 0xf0);

		tp28xx_write_reg(ctx,0x02, 0x42);
		tp28xx_write_reg(ctx,0x07, 0xc0); 
		tp28xx_write_reg(ctx,0x0b, 0xc0);  		
		tp28xx_write_reg(ctx,0x0c, 0x13); 
		tp28xx_write_reg(ctx,0x0d, 0x50);  

		tp28xx_write_reg(ctx,0x15, 0x13);
		tp28xx_write_reg(ctx,0x16, 0x15); 
		tp28xx_write_reg(ctx,0x17, 0x00); 
		tp28xx_write_reg(ctx,0x18, 0x90);
		tp28xx_write_reg(ctx,0x19, 0xd0);
		tp28xx_write_reg(ctx,0x1a, 0x25);			
		tp28xx_write_reg(ctx,0x1c, 0x06);  
		tp28xx_write_reg(ctx,0x1d, 0x72);

		tp28xx_write_reg(ctx,0x20, 0x30);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x2b, 0x60);
		tp28xx_write_reg(ctx,0x2c, 0x0a);
		tp28xx_write_reg(ctx,0x2d, 0x30);
		tp28xx_write_reg(ctx,0x2e, 0x70);

		tp28xx_write_reg(ctx,0x30, 0x48);
		tp28xx_write_reg(ctx,0x31, 0xba);
		tp28xx_write_reg(ctx,0x32, 0x2e);
		tp28xx_write_reg(ctx,0x33, 0x90);

		tp28xx_write_reg(ctx,0x35, 0x13);
		tp28xx_write_reg(ctx,0x36, 0xe8);
		tp28xx_write_reg(ctx,0x38, 0x00);
		tp28xx_write_reg(ctx,0x39, 0x18);

	}
	else if(HD50 == fmt)
	{
		tmp = tp28xx_read_reg(ctx,0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(ctx,0xf5, tmp);
	
		tp28xx_write_reg(ctx,0x02, 0x42);
		tp28xx_write_reg(ctx,0x07, 0xc0);
		tp28xx_write_reg(ctx,0x0b, 0xc0);
		tp28xx_write_reg(ctx,0x0c, 0x13);
		tp28xx_write_reg(ctx,0x0d, 0x50);
		tp28xx_write_reg(ctx,0x15, 0x13);
		tp28xx_write_reg(ctx,0x16, 0x15);
		tp28xx_write_reg(ctx,0x17, 0x00);
		tp28xx_write_reg(ctx,0x18, 0x19);
		tp28xx_write_reg(ctx,0x19, 0xd0);
		tp28xx_write_reg(ctx,0x1a, 0x25);
		tp28xx_write_reg(ctx,0x1c, 0x07);  //1280*720, 
		tp28xx_write_reg(ctx,0x1d, 0xbc);  //1280*720, 50fps

		tp28xx_write_reg(ctx,0x20, 0x30);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x2b, 0x60);
		tp28xx_write_reg(ctx,0x2c, 0x0a);
		tp28xx_write_reg(ctx,0x2d, 0x30);
		tp28xx_write_reg(ctx,0x2e, 0x70);

		tp28xx_write_reg(ctx,0x30, 0x48);
		tp28xx_write_reg(ctx,0x31, 0xbb);
		tp28xx_write_reg(ctx,0x32, 0x2e);
		tp28xx_write_reg(ctx,0x33, 0x90);

		tp28xx_write_reg(ctx,0x35, 0x05);
		tp28xx_write_reg(ctx,0x38, 0x00);
		tp28xx_write_reg(ctx,0x39, 0x1c);

	}
	else if(HD60 == fmt)
	{
		tmp = tp28xx_read_reg(ctx,0xf5);
		tmp &= ~SYS_MODE[ch];
		tp28xx_write_reg(ctx,0xf5, tmp);
	
		tp28xx_write_reg(ctx,0x02, 0x42);
		tp28xx_write_reg(ctx,0x07, 0xc0);
		tp28xx_write_reg(ctx,0x0b, 0xc0);
		tp28xx_write_reg(ctx,0x0c, 0x13);
		tp28xx_write_reg(ctx,0x0d, 0x50);

		tp28xx_write_reg(ctx,0x15, 0x13);
		tp28xx_write_reg(ctx,0x16, 0x15);
		tp28xx_write_reg(ctx,0x17, 0x00);
		tp28xx_write_reg(ctx,0x18, 0x19);
		tp28xx_write_reg(ctx,0x19, 0xd0);
		tp28xx_write_reg(ctx,0x1a, 0x25);
		tp28xx_write_reg(ctx,0x1c, 0x06);  //1280*720, 
		tp28xx_write_reg(ctx,0x1d, 0x72);  //1280*720, 60fps

		tp28xx_write_reg(ctx,0x20, 0x30);
		tp28xx_write_reg(ctx,0x21, 0x84);
		tp28xx_write_reg(ctx,0x22, 0x36);
		tp28xx_write_reg(ctx,0x23, 0x3c);

		tp28xx_write_reg(ctx,0x2b, 0x60);
		tp28xx_write_reg(ctx,0x2c, 0x0a);
		tp28xx_write_reg(ctx,0x2d, 0x30);
		tp28xx_write_reg(ctx,0x2e, 0x70);

		tp28xx_write_reg(ctx,0x30, 0x48);
		tp28xx_write_reg(ctx,0x31, 0xbb);
		tp28xx_write_reg(ctx,0x32, 0x2e);
		tp28xx_write_reg(ctx,0x33, 0x90);

		tp28xx_write_reg(ctx,0x35, 0x05);
		tp28xx_write_reg(ctx,0x38, 0x00);
		tp28xx_write_reg(ctx,0x39, 0x1c);

	}
}

void TP2855_mipi_out(tw2855_context_t* ctx,unsigned char output)
{
	//mipi setting
	tp28xx_write_reg(ctx,0x40, MIPI_PAGE); //MIPI page
    tp28xx_write_reg(ctx,0x01, 0xf0);
    tp28xx_write_reg(ctx,0x02, 0x01);
    tp28xx_write_reg(ctx,0x08, 0x0f);

	if( MIPI_4CH4LANE_594M == output || MIPI_2CH4LANE_594M == output)
	{

            tp28xx_write_reg(ctx,0x20, 0x44);
            if( MIPI_2CH4LANE_594M == output)  tp28xx_write_reg(ctx,0x20, 0x24);
            tp28xx_write_reg(ctx,0x34, 0xe4); //
            tp28xx_write_reg(ctx,0x15, 0x0C);
            tp28xx_write_reg(ctx,0x25, 0x08);
            tp28xx_write_reg(ctx,0x26, 0x06);
            tp28xx_write_reg(ctx,0x27, 0x11);
            tp28xx_write_reg(ctx,0x29, 0x0a);
            tp28xx_write_reg(ctx,0x33, 0x07);
            tp28xx_write_reg(ctx,0x33, 0x00);
            tp28xx_write_reg(ctx,0x14, 0x33);
            tp28xx_write_reg(ctx,0x14, 0xb3);
            tp28xx_write_reg(ctx,0x14, 0x33);

	
	}
	else if( MIPI_4CH4LANE_297M == output)
	{

            tp28xx_write_reg(ctx,0x20, 0x44);
            tp28xx_write_reg(ctx,0x34, 0xe4); //
            tp28xx_write_reg(ctx,0x14, 0x44);
            tp28xx_write_reg(ctx,0x15, 0x0d);
            tp28xx_write_reg(ctx,0x25, 0x04);
            tp28xx_write_reg(ctx,0x26, 0x03);
            tp28xx_write_reg(ctx,0x27, 0x09);
            tp28xx_write_reg(ctx,0x29, 0x02);
            tp28xx_write_reg(ctx,0x33, 0x07);
            tp28xx_write_reg(ctx,0x33, 0x00);
            tp28xx_write_reg(ctx,0x14, 0xc4);
            tp28xx_write_reg(ctx,0x14, 0x44);
	}
	else if( MIPI_1CH2LANE_297M == output)
	{
            tp28xx_write_reg(ctx,0x20, 0x12);// 1ch 2lane
            tp28xx_write_reg(ctx,0x34, 0xe4); //use vin1
            tp28xx_write_reg(ctx,0x14, 0x54);//csi half
            tp28xx_write_reg(ctx,0x15, 0x0d);
            tp28xx_write_reg(ctx,0x25, 0x04);
            tp28xx_write_reg(ctx,0x26, 0x03);
            tp28xx_write_reg(ctx,0x27, 0x09);
            tp28xx_write_reg(ctx,0x29, 0x02);
            tp28xx_write_reg(ctx,0x33, 0x07);
            tp28xx_write_reg(ctx,0x33, 0x00);
            tp28xx_write_reg(ctx,0x14, 0xD4);//csi half
            tp28xx_write_reg(ctx,0x14, 0x54);//csi half
	
	}
	else if( MIPI_4CH2LANE_594M == output)
	{

            tp28xx_write_reg(ctx,0x20, 0x42);
            if (ctx->dev_index == 1) {
            	tp28xx_write_reg(ctx,0x34, 0x4e); //output vin1&vin2
            }
            else {
            	tp28xx_write_reg(ctx,0x34, 0xe4); //output vin1&vin2
            }
            tp28xx_write_reg(ctx,0x15, 0x0c);
            tp28xx_write_reg(ctx,0x25, 0x08);
            tp28xx_write_reg(ctx,0x26, 0x06);
            tp28xx_write_reg(ctx,0x27, 0x11);
            tp28xx_write_reg(ctx,0x29, 0x0a);
            tp28xx_write_reg(ctx,0x33, 0x07);
            tp28xx_write_reg(ctx,0x33, 0x00);
            tp28xx_write_reg(ctx,0x14, 0x43);
            tp28xx_write_reg(ctx,0x14, 0xc3);
            tp28xx_write_reg(ctx,0x14, 0x43);
            //tp28xx_write_reg(ctx,0x23, 0x03);
            //tp28xx_write_reg(ctx,0x23, 0x00);
	
	}
	else if( MIPI_4CH4LANE_445M == output) //only for 4x960p25/30
	{

            tp28xx_write_reg(ctx,0x20, 0x44);
            tp28xx_write_reg(ctx,0x34, 0xe4); //
            tp28xx_write_reg(ctx,0x12, 0x5f);
            tp28xx_write_reg(ctx,0x13, 0x07);            
            tp28xx_write_reg(ctx,0x15, 0x0C);
            tp28xx_write_reg(ctx,0x25, 0x06);
            tp28xx_write_reg(ctx,0x26, 0x05);
            tp28xx_write_reg(ctx,0x27, 0x0d);
            tp28xx_write_reg(ctx,0x29, 0x0a);
            tp28xx_write_reg(ctx,0x33, 0x07);
            tp28xx_write_reg(ctx,0x33, 0x00);
            tp28xx_write_reg(ctx,0x14, 0x33);
            tp28xx_write_reg(ctx,0x14, 0xb3);
            tp28xx_write_reg(ctx,0x14, 0x33);
	}
	else if( MIPI_4CH4LANE_396M == output) //only for Sony ISX019
	{
            tp28xx_write_reg(ctx,0x20, 0x44);
            tp28xx_write_reg(ctx,0x34, 0xe4); //
            tp28xx_write_reg(ctx,0x12, 0x6a);
            tp28xx_write_reg(ctx,0x13, 0x27);            
            tp28xx_write_reg(ctx,0x15, 0x0C);
            tp28xx_write_reg(ctx,0x25, 0x06);
            tp28xx_write_reg(ctx,0x26, 0x04);
            tp28xx_write_reg(ctx,0x27, 0x0c);
            tp28xx_write_reg(ctx,0x29, 0x0a);
            tp28xx_write_reg(ctx,0x33, 0x07);
            tp28xx_write_reg(ctx,0x33, 0x00);
            tp28xx_write_reg(ctx,0x14, 0x33);
            tp28xx_write_reg(ctx,0x14, 0xb3);
            tp28xx_write_reg(ctx,0x14, 0x33);
	}	
	/* Enable MIPI CSI2 output */
	tp28xx_write_reg(ctx,0x23, 0x02);
	tp28xx_write_reg(ctx,0x23, 0x00);
		
}

int TP2855_channel_init(tw2855_context_t* ctx, uint8_t channel)
{
		/* Disable MIPI CSI2 output */
		tp28xx_write_reg(ctx,0x40, MIPI_PAGE);
		tp28xx_write_reg(ctx,0x23, 0x02);
		if(ctx->std == SD_PAL){
			TP2855_decoder_init(ctx,channel,PAL,STD_TVI);
			logmsg( "Init tp2855 to [TP2855-PAL25-720]\n");
		}else if(ctx->std == HD_720P){
			if(ctx->fps == 30){
				TP2855_decoder_init(ctx,channel,HD30,STD_HDA);
				logmsg( "Init tp2855 to [TP2855-AHD30-720P]\n");
			}else if(ctx->fps == 25){
				TP2855_decoder_init(ctx,channel,HD25,STD_HDA);
				logmsg( "Init tp2855 to [TP2855-AHD25-720P]\n");
			}
			else{
				logmsg( "Please check fps.\n");
			}
		}else{
			logmsg( "Please check std.\n");
		}

//		if(mipi_lanes == 2){
//			TP2855_mipi_out(ctx, MIPI_4CH2LANE_594M);
//		}
//		else{
//			TP2855_mipi_out(ctx, MIPI_4CH4LANE_297M);
//		}
		return 0;
}

